﻿#include "include.h"
#include "locomotion_header.h"
#include "can.h"
#include "gait_math.h"
#include "adrc.h"
#include "base_struct.h"
#if RUN_WEBOTS
#include "wbInterface.h"
#endif
//测量陀螺仪数据    弧度制
 
float FLT_ATT_RT = 0;//不为0翻转站立无法正常
void subscribe_imu_to_webot(robotTypeDef* rob,float dt)
{

	const double* data = wb_inertial_unit_get_roll_pitch_yaw(IMU);
	rob->IMU_now_o.roll = 90 + deg(data[0]);
	rob->IMU_now_o.yaw = deg(data[2]);
	rob->IMU_now_o.pitch = deg(data[1]);

	rob->IMU_now.roll = 90 + deg(data[0]);
	rob->IMU_now.yaw = To_180_degreesw(deg(data[2]) - 90);
	rob->IMU_now.pitch = deg(data[1]);

	rob->IMU_dot.roll = (rob->IMU_now.roll - rob->IMU_last.roll) / dt;
	if(fabs(rob->IMU_now.yaw - rob->IMU_last.yaw)<180)
		rob->IMU_dot.yaw = (rob->IMU_now.yaw - rob->IMU_last.yaw) / dt;
	rob->IMU_dot.pitch = (rob->IMU_now.pitch - rob->IMU_last.pitch) / dt;

	rob->IMU_last.roll  = rob->IMU_now.roll;
	rob->IMU_last.yaw   = rob->IMU_now.yaw;
	rob->IMU_last.pitch = rob->IMU_now.pitch;
	//printf("pit =%f rol=%f yaw=%f ||dpit =%f drol=%f dyaw=%f\n", rob->IMU_now.pitch, rob->IMU_now.roll,rob->IMU_now.yaw,rob->IMU_dot.pitch, rob->IMU_dot.roll, rob->IMU_dot.yaw);
	rob->now_att = rob->IMU_now;
	rob->now_rate = rob->IMU_dot;


	vmc_all.att[PITr]= robotwb.IMU_now.pitch;
	vmc_all.att[ROLr]= robotwb.IMU_now.roll;
	vmc_all.att[YAWr]= robotwb.IMU_now.yaw;

	vmc_all.att_rate[PITr]=robotwb.IMU_dot.pitch;
	vmc_all.att_rate[ROLr]=robotwb.IMU_dot.roll;
	vmc_all.att_rate[YAWr]=robotwb.IMU_dot.yaw;

	static float att_rt_use[3] = {0};
	static char flip_flag_reg = 0;
	if (vmc_all.side_flip != flip_flag_reg) {
		att_rt_use[PITr] = vmc_all.att[PITr];
		att_rt_use[ROLr] = vmc_all.att[ROLr];
		att_rt_use[YAWr] = vmc_all.att[YAWr];
	}
	DigitalLPF(vmc_all.att[PITr], &att_rt_use[PITr], FLT_ATT_RT, dt);
	DigitalLPF(vmc_all.att[ROLr], &att_rt_use[ROLr], FLT_ATT_RT, dt);
	DigitalLPF(vmc_all.att[YAWr], &att_rt_use[YAWr], FLT_ATT_RT, dt);
	flip_flag_reg = vmc_all.side_flip;
	//printf("pit =%f rol=%f yaw=%f\n", vmc_all.att[PITr], vmc_all.att[ROLr], vmc_all.att[YAWr]);
	att_rt_use[YAWr] = 0;
	vmc_all.Rn_b[0][0] = cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
	vmc_all.Rn_b[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
	vmc_all.Rn_b[2][0] = sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

	vmc_all.Rn_b[0][1] = cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	vmc_all.Rn_b[1][1] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	vmc_all.Rn_b[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

	vmc_all.Rn_b[0][2] = -sind(-att_rt_use[PITr]);
	vmc_all.Rn_b[1][2] = sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
	vmc_all.Rn_b[2][2] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);

	mat_trans(vmc_all.Rn_b, vmc_all.Rb_n);

	att_rt_use[ROLr] = 0;
	att_rt_use[YAWr] = 0;
	vmc_all.Rn_b_noroll[0][0] = cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
	vmc_all.Rn_b_noroll[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
	vmc_all.Rn_b_noroll[2][0] = sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

	vmc_all.Rn_b_noroll[0][1] = cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	vmc_all.Rn_b_noroll[1][1] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	vmc_all.Rn_b_noroll[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

	vmc_all.Rn_b_noroll[0][2] = -sind(-att_rt_use[PITr]);
	vmc_all.Rn_b_noroll[1][2] = sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
	vmc_all.Rn_b_noroll[2][2] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);

	mat_trans(vmc_all.Rn_b_noroll, vmc_all.Rb_n_noroll);
#if 1
	att_rt_use[PITr] = vmc_all.ground_att_est[PITr] * -1;
	att_rt_use[ROLr] = vmc_all.ground_att_est[ROLr] * -1;
#else
	att_rt_use[PITr] = vmc_all.ground_att_est[PITr] * 1;
	att_rt_use[ROLr] = vmc_all.ground_att_est[ROLr] * 1;
#endif
	att_rt_use[YAWr] = 0;
	vmc_all.Rn_g[0][0] = cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
	vmc_all.Rn_g[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
	vmc_all.Rn_g[2][0] = sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

	vmc_all.Rn_g[0][1] = cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	vmc_all.Rn_g[1][1] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
	vmc_all.Rn_g[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

	vmc_all.Rn_g[0][2] = -sind(-att_rt_use[PITr]);
	vmc_all.Rn_g[1][2] = sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
	vmc_all.Rn_g[2][2] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);

	mat_trans(vmc_all.Rn_g, vmc_all.Rg_n);

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++) {
			robotwb.Rb_n[i][j] = vmc_all.Rb_n[i][j];
			robotwb.Rn_b[i][j] = vmc_all.Rn_b[i][j];
			robotwb.Rb_n_noroll[i][j] = vmc_all.Rb_n_noroll[i][j];
			robotwb.Rn_b_noroll[i][j] = vmc_all.Rn_b_noroll[i][j];
			robotwb.Rg_n[i][j] = vmc_all.Rg_n[i][j];
			robotwb.Rn_g[i][j] = vmc_all.Rn_g[i][j];
		}
}

	//convert acc
	Vect3 body_acc, world_acc;
#if 1
	const double* acc_data = wb_accelerometer_get_values(ACC);
	vmc_all.acc[0] = acc_data[0];
	vmc_all.acc[1] = acc_data[1];
	vmc_all.acc[2] = acc_data[2];
	//printf("acc=%.3f %.3f %.3f\n", vmc_all.acc[0], vmc_all.acc[1], vmc_all.acc[2]);
	body_acc.x = vmc_all.acc_b.x = -vmc_all.acc[0];
	body_acc.y = vmc_all.acc_b.y = vmc_all.acc[1];
	body_acc.z = vmc_all.acc_b.z = vmc_all.acc[2];

	converV_b_to_nw(body_acc, &world_acc);

	vmc_all.acc_n.x = world_acc.x;
	vmc_all.acc_n.y = world_acc.y;
	vmc_all.acc_n.z = world_acc.z;
#endif
	// printf("body  acc=%f %f %f\n",vmc_all.acc_b.x,vmc_all.acc_b.y,vmc_all.acc_b.z);
	// printf("world acc=%f %f %f\n",vmc_all.acc_n.x,vmc_all.acc_n.y,vmc_all.acc_n.z);

	robotwb.now_att = robotwb.IMU_now;
	robotwb.now_rate = robotwb.IMU_dot;
}

void subscribe_webot_to_vmc(float dt)//将传感器数据赋值给VMC
{
  int i=0,j=0;
  for(i=0;i<2;i++)
  {
		leg_motor[i].t_to_i[0]=pos_force_p.t_to_i;//设置扭矩系数<<-----------------------------
		leg_motor[i].t_to_i[1]=leg_motor[i].t_to_i[0];
		leg_motor[i].t_to_i[2] = leg_motor[i].t_to_i[0];
		
		#if Q_NOW_USE_SET
		vmc[i].sita1=vmc[i].tar_sita1;
		vmc[i].sita2=vmc[i].tar_sita2;
		#else
		if(vmc[i].param.q_now_use_tar){//使用期望作为当前反馈  舵狗模式
			vmc[i].sita1 = vmc[i].tar_sita1;
			vmc[i].sita2 = vmc[i].tar_sita2;
			vmc[i].sita3 = vmc[i].tar_sita3;
			vmc[i].sita4 = vmc[i].tar_sita4;
			vmc[i].sita5 = vmc[i].tar_sita5;
		}else{
			vmc[i].sita1 =  robotwb.Leg[i].sita[0];
			vmc[i].sita2 =  robotwb.Leg[i].sita[1];
			vmc[i].sita3 =  robotwb.Leg[i].sita[2];
			vmc[i].sita4 =  robotwb.Leg[i].sita[3];
			vmc[i].sita5 =  robotwb.Leg[i].sita[4];
			vmc[i].dsita1=  robotwb.Leg[i].sita_d[0];
			vmc[i].dsita2=  robotwb.Leg[i].sita_d[1];	
			vmc[i].dsita3 = robotwb.Leg[i].sita_d[2];
			vmc[i].dsita4 = robotwb.Leg[i].sita_d[3];
			vmc[i].dsita5 = robotwb.Leg[i].sita_d[4];
			//printf("%f %f\n", vmc[i].sita1, vmc[i].sita2);
		}
		#endif

	vmc[i].delta_ht=vmc_robot_p.sw_deltah;
    robotwb.Leg[i].is_ground= vmc[i].ground;
    robotwb.Leg[i].trig_state=vmc[i].param.trig_state;
		
    #if !GROUND_USE_EST//使用着地传感器
    vmc[i].is_touch=robotwb.Leg[i].is_touch;
    #else
    vmc[i].is_touch=robotwb.Leg[i].is_touch_est;
    #endif
		//robotwb.Leg[i].limit_tao[0]=pos_force_p.max_t;
		//robotwb.Leg[i].limit_tao[1]=pos_force_p.max_t;  
		//robotwb.Leg[i].limit_tao[2] = pos_force_p.max_t;
		robotwb.Leg[i].f_pos_pid[Xr]=pos_force_p.f_pos_pid_st[Xr]; 
		robotwb.Leg[i].f_pos_pid[Yr] = pos_force_p.f_pos_pid_st[Yr];
		robotwb.Leg[i].f_pos_pid[Zr]=pos_force_p.f_pos_pid_st[Zr]; 
		robotwb.Leg[i].f_pid[Xr]=pos_force_p.f_pid_st[Xr]; 
		robotwb.Leg[i].f_pid[Yr]=pos_force_p.f_pid_st[Yr];
		robotwb.Leg[i].f_pid[Zr]=pos_force_p.f_pid_st[Zr]; 
		
		//关节PD参数
		if(vmc_all.gait_mode==TROT||vmc_all.gait_mode==F_TROT || vmc_all.gait_mode == WALK)
		{
				if(stand_force_enable_flag[i]){
				robotwb.Leg[i].q_pid=pos_force_p.q_pid_st_trot; 	
				}	
				else
				{
				robotwb.Leg[i].q_pid=pos_force_p.q_pid_sw;//摆动参数
				}		
		}else{
				if(robotwb.Leg[i].is_ground){//支撑参数
				robotwb.Leg[i].q_pid=pos_force_p.q_pid_st_stance; 	
				}
				else
				{
				robotwb.Leg[i].q_pid=pos_force_p.q_pid_sw;//摆动参数
				}
		}
		leg_motor[i].max_t[0]=leg_motor[i].max_t[1]=leg_motor[i].max_t[2]=pos_force_p.max_t;
		leg_motor[i].max_i[0]=leg_motor[i].max_i[1]=leg_motor[i].max_i[2]=pos_force_p.max_i;
  }
	
 //地形角期望命令
 vmc_all.ground_att_cmd[PITr] = vmc_all.ground_att_est[PITr];//在trot模式下是否通过步态相序触发 否则高台阶会不正常
 vmc_all.ground_att_cmd[ROLr] = -vmc_all.ground_att_est[ROLr];
  robotwb.exp_att.pitch=vmc_all.tar_att[PITr]+LIMIT(vmc_all.ground_att_cmd[PITr]*(vmc_all.gait_mode>0)*EN_ATT_GROUND_CONTROL,-35,35);
  robotwb.exp_att.roll= vmc_all.tar_att[ROLr]+LIMIT(vmc_all.ground_att_cmd[ROLr]*(vmc_all.gait_mode>0)*EN_ATT_GROUND_CONTROL,-15,15);
 // printf("robotwb.exp_att.roll=%f\n", robotwb.exp_att.roll);
}

void subscribe_webot_to_vmc1(float dt)//将传感器数据赋值给VMC
{
	int i = 0, j = 0;
	for (i = 0; i < 2; i++)
	{

#if Q_NOW_USE_SET
		vmc[i].sita1 = vmc[i].tar_sita1;
		vmc[i].sita2 = vmc[i].tar_sita2;
#else
		if (vmc[i].param.q_now_use_tar) {//使用期望作为当前反馈  舵狗模式
			vmc[i].sita1 = vmc[i].tar_sita1;
			vmc[i].sita2 = vmc[i].tar_sita2;
			vmc[i].sita3 = vmc[i].tar_sita3;
			vmc[i].sita4 = vmc[i].tar_sita4;
			vmc[i].sita5 = vmc[i].tar_sita5;
		}
		else {
			vmc[i].sita1 = robotwb.Leg[i].sita[0];
			vmc[i].sita2 = robotwb.Leg[i].sita[1];
			vmc[i].sita3 = robotwb.Leg[i].sita[2];
			vmc[i].sita4 = robotwb.Leg[i].sita[3];
			vmc[i].sita5 = robotwb.Leg[i].sita[4];
			vmc[i].dsita1 = robotwb.Leg[i].sita_d[0];
			vmc[i].dsita2 = robotwb.Leg[i].sita_d[1];
			vmc[i].dsita3 = robotwb.Leg[i].sita_d[2];
			vmc[i].dsita4 = robotwb.Leg[i].sita_d[3];
			vmc[i].dsita5 = robotwb.Leg[i].sita_d[4];
			//printf("%f %f\n", vmc[i].sita1, vmc[i].sita2);
		}
#endif
	}
}

void subscribe_webot_to_vmc2(float dt)//将传感器数据赋值给VMC
{
	int i = 0, j = 0;
	for (i = 0; i < 2; i++)
	{
		leg_motor[i].t_to_i[0] = pos_force_p.t_to_i;//设置扭矩系数<<-----------------------------
		leg_motor[i].t_to_i[1] = leg_motor[i].t_to_i[0];
		leg_motor[i].t_to_i[2] = leg_motor[i].t_to_i[0];
		vmc[i].delta_ht = vmc_robot_p.sw_deltah;
		robotwb.Leg[i].is_ground = vmc[i].ground;
		robotwb.Leg[i].trig_state = vmc[i].param.trig_state;

		//robotwb.Leg[i].limit_tao[0] = pos_force_p.max_t;
		//robotwb.Leg[i].limit_tao[1] = pos_force_p.max_t;
		//robotwb.Leg[i].limit_tao[2] = pos_force_p.max_t;
		robotwb.Leg[i].f_pos_pid[Xr] = pos_force_p.f_pos_pid_st[Xr];
		robotwb.Leg[i].f_pos_pid[Yr] = pos_force_p.f_pos_pid_st[Yr];
		robotwb.Leg[i].f_pos_pid[Zr] = pos_force_p.f_pos_pid_st[Zr];
		robotwb.Leg[i].f_pid[Xr] = pos_force_p.f_pid_st[Xr];
		robotwb.Leg[i].f_pid[Yr] = pos_force_p.f_pid_st[Yr];
		robotwb.Leg[i].f_pid[Zr] = pos_force_p.f_pid_st[Zr];

		//关节PD参数
		if (vmc_all.gait_mode == TROT || vmc_all.gait_mode == F_TROT || vmc_all.gait_mode == WALK || vmc_all.gait_mode == ROLLING)
		{
			if (robotwb.Leg[i].is_ground) {//stand_force_enable_flag[i]){
				robotwb.Leg[i].q_pid = pos_force_p.q_pid_st_trot;
			}
			else
			{
				robotwb.Leg[i].q_pid = pos_force_p.q_pid_sw;//摆动参数
			}
		}
		else {
			if (robotwb.Leg[i].is_ground) {//支撑参数
				robotwb.Leg[i].q_pid = pos_force_p.q_pid_st_stance;
			}
			else
			{
				robotwb.Leg[i].q_pid = pos_force_p.q_pid_sw;//摆动参数
			}
		}
		leg_motor[i].max_t[0] = leg_motor[i].max_t[1] = leg_motor[i].max_t[2] = pos_force_p.max_t;
		leg_motor[i].max_i[0] = leg_motor[i].max_i[1] = leg_motor[i].max_i[2] = pos_force_p.max_i;
	}
}

void subscribe_webot_to_vmc3(float dt)//将传感器数据赋值给VMC
{
	int i = 0, j = 0;
	for (i = 0; i < 2; i++)
	{
#if !GROUND_USE_EST//使用着地传感器
		vmc[i].is_touch = robotwb.Leg[i].is_touch;
#else
		vmc[i].is_touch = robotwb.Leg[i].is_touch_est;
#endif
	}

	//地形角期望命令
	vmc_all.ground_att_cmd[PITr] = vmc_all.ground_att_est[PITr];
	vmc_all.ground_att_cmd[ROLr] = -vmc_all.ground_att_est[ROLr];
	robotwb.exp_att.pitch = vmc_all.tar_att[PITr] + LIMIT(vmc_all.ground_att_cmd[PITr] * (vmc_all.gait_mode > 0)*EN_ATT_GROUND_CONTROL, -35, 35);
	robotwb.exp_att.roll =  vmc_all.tar_att[ROLr] + LIMIT(vmc_all.ground_att_cmd[ROLr] * (vmc_all.gait_mode > 0)*EN_ATT_GROUND_CONTROL, -25, 25);
}

void subscribe_webot_to_vmc4(float dt)//将传感器数据赋值给VMC 目前使用
{
	int i = 0, j = 0;

	for (i = 0; i < 2; i++)
	{
		vmc[i].delta_ht = vmc_robot_p.sw_deltah;
	}
	//地形角期望命令
	vmc_all.ground_att_cmd[PITr] = vmc_all.ground_att_est[PITr];
	vmc_all.ground_att_cmd[ROLr] = -vmc_all.ground_att_est[ROLr];
	//("p=%f\n", vmc_all.ground_att_cmd[PITr]);
	robotwb.exp_att.pitch = vmc_all.tar_att[PITr] + LIMIT(vmc_all.ground_att_cmd[PITr] * (vmc_all.gait_mode > 0)*EN_ATT_GROUND_CONTROL, -35, 35);
	robotwb.exp_att.roll = vmc_all.tar_att[ROLr] + LIMIT(dead(vmc_all.ground_att_cmd[ROLr],8)  * (vmc_all.gait_mode > 0)*EN_ATT_GROUND_CONTROL, -25, 25);
}


void publish_vmc_to_webot(float dt)
{
    int i=0;
    for(i=0;i<2;i++){
    float alfa=(robotwb.Leg[i].sita[1]-robotwb.Leg[i].sita[0])/2;
	float beta=(robotwb.Leg[i].sita[1]+robotwb.Leg[i].sita[0])/2;
	robotwb.Leg[i].beta=beta;
    robotwb.Leg[i].alfa=alfa;

    robotwb.Leg[i].r=vmc[i].r;
    robotwb.Leg[i].sita_r=vmc[i].sita;
    robotwb.Leg[i].epos_h.x=vmc[i].eposc.x;
	robotwb.Leg[i].epos_h.y=vmc[i].eposc.y;
    robotwb.Leg[i].epos_h.z=vmc[i].eposc.z;

	robotwb.Leg[i].epos_b.x=vmc[i].epos_b.x;
	robotwb.Leg[i].epos_b.y=vmc[i].epos_b.y;
	robotwb.Leg[i].epos_b.z=vmc[i].epos_b.z;

	robotwb.Leg[i].epos_n.x=vmc[i].epos_n.x;
	robotwb.Leg[i].epos_n.y=vmc[i].epos_n.y;
	robotwb.Leg[i].epos_n.z=vmc[i].epos_n.z;
	
	robotwb.Leg[i].epos_bc.x = vmc[i].epos_bc.x;
	robotwb.Leg[i].epos_bc.y = vmc[i].epos_bc.y;
	robotwb.Leg[i].epos_bc.z = vmc[i].epos_bc.z;

	robotwb.Leg[i].epos_nc.x = vmc[i].epos_nc.x;
	robotwb.Leg[i].epos_nc.y = vmc[i].epos_nc.y;
	robotwb.Leg[i].epos_nc.z = vmc[i].epos_nc.z;

    robotwb.Leg[i].espd_h.x=vmc[i].spd.x;
	robotwb.Leg[i].espd_h.y=vmc[i].spd.y;
    robotwb.Leg[i].espd_h.z=vmc[i].spd.z;

    robotwb.Leg[i].espd_n.x=vmc[i].spd_n.x;
	robotwb.Leg[i].espd_n.y=vmc[i].spd_n.y;
    robotwb.Leg[i].espd_n.z=vmc[i].spd_n.z;
#if 1
	for (int j = 0; j < 9; j++) {
		robotwb.Leg[i].jacobi[j] = vmc[i].jacobi33[j];
		robotwb.Leg[i].jacobi_inv[j] = vmc[i].ijacobi33[j];
	}
#else
    robotwb.Leg[i].jacobi[0]=vmc[i].jacobi22[0];
    robotwb.Leg[i].jacobi[2]=vmc[i].jacobi22[2];

    robotwb.Leg[i].jacobi[1]=vmc[i].jacobi22[1];
    robotwb.Leg[i].jacobi[3]=vmc[i].jacobi22[3];

    robotwb.Leg[i].jacobi_inv[0]=vmc[i].ijacobi22[0];
    robotwb.Leg[i].jacobi_inv[2]=vmc[i].ijacobi22[2];

    robotwb.Leg[i].jacobi_inv[1]=vmc[i].ijacobi22[1];
    robotwb.Leg[i].jacobi_inv[3]=vmc[i].ijacobi22[3];
#endif
	}
}


/* 读取所有电机转角 */
void readAllMotorPos(robotTypeDef* rob, float dt)
{
	static float cnt;
	uint8_t i, j;
	float taom[4][5] = { 0 };
	cnt += dt;
	for (i = 0; i < 2; i++) {//g 角度反馈
		for (j = 0; j < 5; j++) {
			rob->Leg[i].pos[j] = wb_position_sensor_get_value(posensor1[5 * i + j]);
			taom[i][j] = wb_motor_get_torque_feedback(motor1[5 * i + j]);
		}
	}
	for (i = 0; i < 2; i++) {
		if (i == 0) {
			rob->Leg[i].sita[0] = -(rob->Leg[i].pos[2] * 57.3);//大腿
			rob->Leg[i].sita[1] = -(To_180_degreesw(180-rob->Leg[i].pos[3] * 57.3));//小腿
			rob->Leg[i].sita[2] = (rob->Leg[i].pos[0] * 57.3);//侧摆
			rob->Leg[i].sita[3] = (rob->Leg[i].pos[1] * 57.3);//航向
			rob->Leg[i].sita[4] = (rob->Leg[i].pos[4] * 57.3);//足端
		}
		else {
			rob->Leg[i].sita[0] = (rob->Leg[i].pos[2] * 57.3);//大腿
			rob->Leg[i].sita[1] = (To_180_degreesw(180-rob->Leg[i].pos[3] * 57.3));//小腿
			rob->Leg[i].sita[2] = (rob->Leg[i].pos[0] * 57.3);//侧摆
			rob->Leg[i].sita[3] = (rob->Leg[i].pos[1] * 57.3);//航向
			rob->Leg[i].sita[4] = (rob->Leg[i].pos[4] * 57.3);//足端
		}
		//printf("leg=%d q=%.2f %.2f %.2f | %.2f | %.2f\n", i,  rob->Leg[i].sita[0], rob->Leg[i].sita[1], rob->Leg[i].sita[2], rob->Leg[i].sita[3], rob->Leg[i].sita[4]);
		DigitalLPFw(LIMIT(To_180_degreesw(rob->Leg[i].sita[0] - rob->Leg[i].sita_reg[0]) / dt,-200,200), &rob->Leg[i].sita_d[0], 0, dt);
		DigitalLPFw(LIMIT(To_180_degreesw(rob->Leg[i].sita[1] - rob->Leg[i].sita_reg[1]) / dt, -200, 200), &rob->Leg[i].sita_d[1], 0, dt);
		DigitalLPFw(LIMIT(To_180_degreesw(rob->Leg[i].sita[2] - rob->Leg[i].sita_reg[2]) / dt, -200, 200), &rob->Leg[i].sita_d[2], 0, dt);
		DigitalLPFw(LIMIT(To_180_degreesw(rob->Leg[i].sita[3] - rob->Leg[i].sita_reg[3]) / dt, -200, 200), &rob->Leg[i].sita_d[3], 0, dt);
		DigitalLPFw(LIMIT(To_180_degreesw(rob->Leg[i].sita[4] - rob->Leg[i].sita_reg[4]) / dt, -200, 200), &rob->Leg[i].sita_d[4], 0, dt);

		rob->Leg[i].sita_reg[0] = rob->Leg[i].sita[0];
		rob->Leg[i].sita_reg[1] = rob->Leg[i].sita[1];
		rob->Leg[i].sita_reg[2] = rob->Leg[i].sita[2];
		rob->Leg[i].sita_reg[3] = rob->Leg[i].sita[3];
		rob->Leg[i].sita_reg[4] = rob->Leg[i].sita[4];
	}
}

#if !RUN_WEBOTS
#else
void pos_control_pd(float dt)
{
	int id = 3;
	for (id = 0; id < 2; id++) {//测试角度坐标系
		if (vmc_all.side_flip) {
			robotwb.Leg[id].tar_sita[0] = -0;
			robotwb.Leg[id].tar_sita[1] = 180;

			robotwb.Leg[id].q_pid.kp = 0.05;
			robotwb.Leg[id].q_pid.kd = 0.001;
		}
		robotwb.Leg[id].tar_sita[0] = limitw(robotwb.Leg[id].tar_sita[0], -robotwb.Leg[id].limit_sita[0], 180 + robotwb.Leg[id].limit_sita[0]);
		robotwb.Leg[id].tar_sita[1] = limitw(robotwb.Leg[id].tar_sita[1], -robotwb.Leg[id].limit_sita[1], 180 + robotwb.Leg[id].limit_sita[1]);
		robotwb.Leg[id].taod[0] = (To_180_degreesw(robotwb.Leg[id].tar_sita[0] - robotwb.Leg[id].sita[0])*robotwb.Leg[id].q_pid.kp - robotwb.Leg[id].sita_d[0] * robotwb.Leg[id].q_pid.kd) * 1;
		robotwb.Leg[id].taod[1] = (To_180_degreesw(robotwb.Leg[id].tar_sita[1] - robotwb.Leg[id].sita[1])*robotwb.Leg[id].q_pid.kp - robotwb.Leg[id].sita_d[1] * robotwb.Leg[id].q_pid.kd) * 1;
		//printf("leg=%d s0=%f %f s1=%f %f ds=%f %f\n",id,robotwb.Leg[id].tar_sita[0],robotwb.Leg[id].sita[0],robotwb.Leg[id].tar_sita[1],robotwb.Leg[id].sita[1],robotwb.Leg[id].sita_d[0],robotwb.Leg[id].sita_d[1]);
		set_motor_t(id);
	}
}


void set_motor_q(int id)//g
{
	float tar_sita[5] = { 0 };

	tar_sita[0] = limitw(robotwb.Leg[id].tar_sita[0], -45, robotwb.Leg[id].limit_sita[0]);//大腿
	tar_sita[1] = limitw(robotwb.Leg[id].tar_sita[1],  10, robotwb.Leg[id].limit_sita[1]);//小腿
	tar_sita[2] = limitw(robotwb.Leg[id].tar_sita[2], -robotwb.Leg[id].limit_sita[2], robotwb.Leg[id].limit_sita[2]);//侧展
	tar_sita[3] = limitw(robotwb.Leg[id].tar_sita[3], -robotwb.Leg[id].limit_sita[3], robotwb.Leg[id].limit_sita[3]);//航向
	tar_sita[4] = limitw(robotwb.Leg[id].tar_sita[4], -robotwb.Leg[id].limit_sita[4], robotwb.Leg[id].limit_sita[4]);//足端

	switch (id) {
	case 0://右腿
		wb_motor_set_position(motor1[0], rad(tar_sita[2]));//侧展
		wb_motor_set_position(motor1[1], rad(tar_sita[3]));//航向
		wb_motor_set_position(motor1[2], rad(-tar_sita[0]));//大腿
		wb_motor_set_position(motor1[3], rad(-To_180_degreesw(180-tar_sita[1])));//小腿
		wb_motor_set_position(motor1[4], rad(tar_sita[4]));//足端
		break;
	case 1://左腿
		wb_motor_set_position(motor1[5], rad(tar_sita[2]));
		wb_motor_set_position(motor1[6], rad(tar_sita[3]));
		wb_motor_set_position(motor1[7], rad(tar_sita[0]));
		wb_motor_set_position(motor1[8], rad(To_180_degreesw(180 - tar_sita[1])));
		wb_motor_set_position(motor1[9], rad(tar_sita[4]));
		break;
	}
}


void set_motor_q_sel(int id)//g
{
	float tar_sita[5] = { 0 };
	tar_sita[3] = limitw(robotwb.Leg[id].tar_sita[3], -robotwb.Leg[id].limit_sita[3], robotwb.Leg[id].limit_sita[3]);//航向
	tar_sita[4] = limitw(robotwb.Leg[id].tar_sita[4], -robotwb.Leg[id].limit_sita[4], robotwb.Leg[id].limit_sita[4]);//足端
	switch (id) {
	case 0://右腿
		wb_motor_set_position(motor1[1], rad(tar_sita[3]));//航向
		//wb_motor_set_position(motor1[4], rad(tar_sita[4]));//足端
		break;
	case 1://左腿
		wb_motor_set_position(motor1[6], rad(tar_sita[3]));
		//wb_motor_set_position(motor1[9], rad(tar_sita[4]));
		break;
	}
}


void set_motor_q_up(int id)//g
{
	float tar_sita[5] = { 0 };

	tar_sita[0] = limitw(robotwb.Leg[id].tar_sita_up[0], -65, 65);//侧展
	tar_sita[1] = limitw(robotwb.Leg[id].tar_sita_up[1], -90, 90);//大臂
	tar_sita[2] = limitw(robotwb.Leg[id].tar_sita_up[2], 0, 180);//小臂

	switch (id) {
	case 0://右臂
		wb_motor_set_position(motor1_up[0], -rad(tar_sita[0]));//侧展
		wb_motor_set_position(motor1_up[1], -rad(tar_sita[1]));//航向
		wb_motor_set_position(motor1_up[2], -rad(To_180_degreesw(180 - tar_sita[2])));//大腿
		break;
	case 1://左臂
		wb_motor_set_position(motor1_up[3], -rad(tar_sita[0]));
		wb_motor_set_position(motor1_up[4], rad(tar_sita[1]));
		wb_motor_set_position(motor1_up[5], rad(To_180_degreesw(180 - tar_sita[2])));
		break;
	}
}

#if 1

void set_motor_t(int id)//g
{
	robotwb.Leg[id].taod[0] = limitw(robotwb.Leg[id].taod[0], -robotwb.Leg[id].limit_tao[0], robotwb.Leg[id].limit_tao[0]);
	robotwb.Leg[id].taod[1] = limitw(robotwb.Leg[id].taod[1], -robotwb.Leg[id].limit_tao[1], robotwb.Leg[id].limit_tao[1]);
	robotwb.Leg[id].taod[2] = limitw(robotwb.Leg[id].taod[2], -robotwb.Leg[id].limit_tao[2], robotwb.Leg[id].limit_tao[2]);
	robotwb.Leg[id].taod[3] = limitw(robotwb.Leg[id].taod[3], -robotwb.Leg[id].limit_tao[3], robotwb.Leg[id].limit_tao[3]);
	robotwb.Leg[id].taod[4] = limitw(robotwb.Leg[id].taod[4], -robotwb.Leg[id].limit_tao[4], robotwb.Leg[id].limit_tao[4]);

	robotwb.Leg[id].taom_output[0] = robotwb.Leg[id].taod[0];
	robotwb.Leg[id].taom_output[1] = robotwb.Leg[id].taod[1];
	robotwb.Leg[id].taom_output[2] = robotwb.Leg[id].taod[2];
	robotwb.Leg[id].taom_output[3] = robotwb.Leg[id].taod[3];
	robotwb.Leg[id].taom_output[4] = robotwb.Leg[id].taod[4];

	switch (id) {
	case 0://右腿
		wb_motor_set_torque(motor1[0], (robotwb.Leg[id].taod[2]));//侧展
		wb_motor_set_torque(motor1[1], (robotwb.Leg[id].taod[3]));//航向
		wb_motor_set_torque(motor1[2], (-robotwb.Leg[id].taod[0]));//大腿
		wb_motor_set_torque(motor1[3], (robotwb.Leg[id].taod[1]));//小腿
		wb_motor_set_torque(motor1[4], (robotwb.Leg[id].taod[4]));//足端
		break;
	case 1://左腿
		wb_motor_set_torque(motor1[5], (robotwb.Leg[id].taod[2]));
		wb_motor_set_torque(motor1[6], (robotwb.Leg[id].taod[3]));
		wb_motor_set_torque(motor1[7], (robotwb.Leg[id].taod[0]));
		wb_motor_set_torque(motor1[8], (-robotwb.Leg[id].taod[1]));
		wb_motor_set_torque(motor1[9], (robotwb.Leg[id].taod[4]));
		break;
	}
}

#endif
void updateJoy(void)//user_xd_d button
{
	float axis_max = 32768.0;
	float FLT_RC = 1;
	float T = 0.005;
	float rate_yaw_w[2];
	int axis_dead = 200;
	int axis = wb_joystick_get_number_of_axes();
	int povs = wb_joystick_get_number_of_povs();
	static float timer_rc = 0;
	static int flag_rc = 0;
	if (wb_joystick_is_connected())
	{
		// printf("joy_is_connected Axis=%d %d\n",wb_joystick_get_number_of_axes(),wb_joystick_get_number_of_povs());
		for (int i = 0; i < axis; i++)
		{
			int axis_v = 0;// dead(wb_joystick_get_axis_value(i), axis_dead);
			float rc_temp = (float)axis_v / axis_max;
			//printf("[%d]axis = %f\n",i,rc_temp);
			if (i == 0)
				robotwb.ocu.rc_spd_w[Xrw] = -rc_temp;

			if (i == 1)
				robotwb.ocu.rc_spd_w[Yrw] = rc_temp;

			if (i == 2)
				robotwb.ocu.rc_att_w[PITrw] = -rc_temp;

			if (i == 3)
				robotwb.ocu.rc_att_w[ROLrw] = rc_temp;

			if (i == 4)
				rate_yaw_w[0] = rc_temp;

			if (i == 5)
				rate_yaw_w[1] = rc_temp;
		}

		robotwb.ocu.rate_yaw_w = rate_yaw_w[0] - rate_yaw_w[1];
		for (int i = 0; i < povs; i++)
		{
			int povs_v = (wb_joystick_get_pov_value(i));
			// printf("[%d]pov = %f\n",i,povs_v);

		}
		int button = wb_joystick_get_pressed_button();
		//printf("button = %d\n",button);
		robotwb.ocu.key_y = 0;
		robotwb.ocu.key_x = 0;
		robotwb.ocu.key_b = 0;
		robotwb.ocu.key_a = 0;
		robotwb.ocu.key_st = 0;
		robotwb.ocu.key_back = 0;
		robotwb.ocu.key_ll = 0;
		robotwb.ocu.key_rr = 0;

		if (button == 11)
			robotwb.ocu.key_y = 1;
		if (button == 10)
			robotwb.ocu.key_x = 1;
		if (button == 9)
			robotwb.ocu.key_b = 1;
		if (button == 8)
			robotwb.ocu.key_a = 1;
		if (button == 4)
			robotwb.ocu.key_ll = 1;
		if (button == 5)
			robotwb.ocu.key_rr = 1;
		if (button == 0)
			robotwb.ocu.key_st = 1;
		if (button == 1)
			robotwb.ocu.key_back = 1;
	}
	else
	{
#if NO_RC&&!EN_HUMAN
		if (vmc_all.gait_mode == TROT|| vmc_all.gait_mode == F_TROT || vmc_all.gait_mode == WALK)
			timer_rc += 0.005;
		else
			timer_rc = 0;
		//printf("timer_rc=%f\n", timer_rc);
		if (timer_rc > FORWARD_REPEAT_TIME)
		{
			timer_rc = 0;
			printf("Speed flag invert!!\n");
			flag_rc = !flag_rc;
			//flag_rc = 99;
		}
#if 1
		//DigitalLPF(ocu.rc_spd_w[Xr] * MAX_SPD, &vmc_all.tar_spd.x, FLT_RC * 4, T);
		//DigitalLPF(ocu.rc_spd_w[Yr] * MAX_SPD, &vmc_all.tar_spd.y, FLT_RC * 4 / 2, T);
		//DigitalLPF(ocu.rate_yaw_w*MAX_SPD_RAD, &vmc_all.tar_spd.z, FLT_RC * 4, T);
		//DigitalLPF(-ocu.rc_att_w[PITr] * vmc_all.param.MAX_PIT, &vmc_all.tar_att[PITr], FLT_RC * 4, T);
		//DigitalLPF(ocu.rc_att_w[ROLr] * vmc_all.param.MAX_ROL, &vmc_all.tar_att[ROLr], FLT_RC * 4, T);
		if (vmc_all.gait_mode == TROT || vmc_all.gait_mode == F_TROT|| vmc_all.gait_mode==G_ETL) {
			if (flag_rc != 99) {
				if (!flag_rc)
					DigitalLPFw(FORWARD_SPD, &vmc_all.tar_spd.x, 0.05, T);
				else
					DigitalLPFw(-FORWARD_SPD, &vmc_all.tar_spd.x, 0.05, T);

				if (!flag_rc)
					DigitalLPFw(FORWARD_SPD_Y, &vmc_all.tar_spd.y, 0.1, T);
				else
					DigitalLPFw(-FORWARD_SPD_Y, &vmc_all.tar_spd.y, 0.1, T);
			}
			else
				vmc_all.tar_spd.x = vmc_all.tar_spd.y = 0;
		}
		else {
			vmc_all.tar_spd.x = 0;
			vmc_all.tar_spd.y = 0;
			//if (!flag_rc)
			//	DigitalLPFw(FORWARD_SPD_WALK, &vmc_all.tar_spd.x, 0.3, T);
			//else
			//	DigitalLPFw(-FORWARD_SPD_WALK, &vmc_all.tar_spd.x, 0.3, T);	
		}
		vmc_all.tar_spd.x *= EN_AUTO_FORWARD_TROT;
		vmc_all.tar_spd.z = -60 * EN_AUTO_ROTATE_TROT;

#endif
#endif
	}

}

void updateKey(void) {
	int key = wb_keyboard_get_key();
	if (key > 0) {
		printf("Key board==%d\n", key);
	}
}


FILE *fp1, *fp2, *fp3,*fp4, *fp5;
#define LEN_RECORD 80//记录位置
float data_record1[LEN_RECORD] = {0}, data_record2[LEN_RECORD] = { 0 }, data_record3[LEN_RECORD] = { 0 }, data_record4[LEN_RECORD] = { 0 }, data_record5[LEN_RECORD] = { 0 };//webots
void recorder(float dt) {
	static int state = 0;
	int i = 0;
	//webots绘图  doghome--------------------
	END_POS tar_epos_n, tar_epos_b, slip_epos_n;
	for (i = 0; i < 4; i++) {

		converV_leg_to_b(i, vmc[i].tar_epos_h.x,
			vmc[i].tar_epos_h.y,
			vmc[i].tar_epos_h.z,
			&tar_epos_b.x, &tar_epos_b.y, &tar_epos_b.z);
		converV_b_to_n(tar_epos_b.x,
			tar_epos_b.y,
			tar_epos_b.z,
			&tar_epos_n.x, &tar_epos_n.y, &tar_epos_n.z);

		if (vmc[i].param.trig_state >= 1 && vmc[i].param.trig_state <= 2) {
			tar_epos_n = vmc[i].tar_epos_n;
			slip_epos_n = vmc[i].slip_epos_n;
		}
		else if (vmc[i].param.trig_state == 3)
			;
		else if (vmc[i].ground) {
			tar_epos_n.z = -99;
			slip_epos_n.z = -99;
		}

		webots_draw.leg_sw_tar[i] = tar_epos_n;
		webots_draw.leg_sw_slip[i] = slip_epos_n;
		


		webots_draw.zmp_pos.x = walk_gait.now_zmp_nn.x- vmc_all.cog_pos_n.x;
		webots_draw.zmp_pos.y = walk_gait.now_zmp_nn.y- vmc_all.cog_pos_n.y;
		webots_draw.zmp_pos.z = -vmc_all.pos_n.z + 0.01;

		webots_draw.tar_zmp_pos.x = walk_gait.tar_zmp_n.x- vmc_all.cog_pos_n.x;
		webots_draw.tar_zmp_pos.y = walk_gait.tar_zmp_n.y- vmc_all.cog_pos_n.y;
		webots_draw.tar_zmp_pos.z = -vmc_all.pos_n.z + 0.01;

		if (_hu_model.stair_slope_mode_flag == 2) {//台阶
			webots_draw.mu_limit[i].x = 1;
			webots_draw.mu_limit[i].y = 0.01;
			webots_draw.mu_limit[i].z = 0.01;
		}
		else {
			webots_draw.mu_limit[i].x = 1 * cosd(vmc_all.ground_att_est[PITr]);
			webots_draw.mu_limit[i].y = 0.01;
			webots_draw.mu_limit[i].z = 1 * sind(vmc_all.ground_att_est[PITr]);
		}
		webots_draw.ground[i] = vmc[i].ground;
		webots_draw.touch[i] = vmc[i].is_touch;

	}

	webots_draw.tar_grf[1].x = webots_draw.tar_grf[0].x = robotwb.Leg[0].tar_force_dis_n.x;
	webots_draw.tar_grf[1].y = webots_draw.tar_grf[0].y = robotwb.Leg[0].tar_force_dis_n.y;
	webots_draw.tar_grf[1].z = webots_draw.tar_grf[0].z = robotwb.Leg[0].tar_force_dis_n.z;

	webots_draw.tar_grf[2].x = webots_draw.tar_grf[3].x = robotwb.Leg[1].tar_force_dis_n.x;
	webots_draw.tar_grf[2].y = webots_draw.tar_grf[3].y = robotwb.Leg[1].tar_force_dis_n.y;
	webots_draw.tar_grf[2].z = webots_draw.tar_grf[3].z = robotwb.Leg[1].tar_force_dis_n.z;
 
	webots_draw.ground[1] = webots_draw.ground[0] = vmc[0].ground;
	webots_draw.ground[2] = webots_draw.ground[3] = vmc[1].ground;
	webots_draw.touch[0] = webots_draw.touch[1] = vmc[0].is_touch;
	webots_draw.touch[2] = webots_draw.touch[3] = vmc[1].is_touch;
 
	//printf("d %f[%d %d] %f[%d %d] %f[%d %d] %f[%d %d]\n",
	//	webots_draw.tar_grf[0].z, 1, vmc[0].is_touch,
	//	webots_draw.tar_grf[1].z, 1, vmc[1].is_touch,
	//	webots_draw.tar_grf[2].z, 1, vmc[2].is_touch,
	//	webots_draw.tar_grf[3].z, 1, vmc[3].is_touch);

	static int cnt=0;
	if (cnt++ > 1||1) {
		cnt = 0;
		share_memory_drawing();//to drawing controller
	}
#if EN_RECORDER
	switch (state)
	{
	case 0:
		fp1 = fopen(".\\Data\\file1.txt", "w");
		fp2 = fopen(".\\Data\\file2.txt", "w");
		fp3 = fopen(".\\Data\\file3.txt", "w");
		fp4 = fopen(".\\Data\\file4.txt", "w");
		fp5 = fopen(".\\Data\\file5.txt", "w");
		if (fp1 != NULL) {
			printf("---------------------------------Record Ready---------------------------------\n");
			state++;
		}
		break;
	case 1:
		for (i = 0; i < LEN_RECORD; i++)
			fprintf(fp1, "%.6f ",//姿态数据
				data_record1[i]);
		fprintf(fp1, "\n");

		for (i = 0; i < LEN_RECORD; i++)
			fprintf(fp2, "%.6f ",//单腿力
				data_record2[i]);
		fprintf(fp2, "\n");

		for (i = 0; i < LEN_RECORD; i++)
			fprintf(fp3, "%.6f ",//单腿运动
				data_record3[i]);
		fprintf(fp3, "\n");

		for (i = 0; i < LEN_RECORD; i++)
			fprintf(fp4, "%.6f ",//单腿运动
				data_record4[i]);
		fprintf(fp4, "\n");

		for (i = 0; i < LEN_RECORD; i++)
			fprintf(fp5, "%.6f ",//单腿运动
				data_record5[i]);
		fprintf(fp5, "\n");
		break;
	case 2:
		fclose(fp1); fclose(fp2); fclose(fp3); fclose(fp4); fclose(fp5);
		printf("---------------------------------Record Finish--------------------------\n");
		break;
	}
#endif
}

void record_thread(char sel, float dt)
{
	int cnt = 0;
	switch (sel) {
	case 0:
		cnt = 0;
		data_record1[cnt++] = robotwb.exp_att.pitch;
		data_record1[cnt++] = robotwb.now_att.pitch;

		data_record1[cnt++] = robotwb.exp_att.roll;
		data_record1[cnt++] = robotwb.now_att.roll;

		data_record1[cnt++] = robotwb.exp_att.yaw;
		data_record1[cnt++] = robotwb.now_att.yaw;

		data_record1[cnt++] = robotwb.now_rate.pitch;
		data_record1[cnt++] = robotwb.now_rate.roll;
		data_record1[cnt++] = robotwb.now_rate.yaw;

		data_record1[cnt++] = robotwb.exp_pos_n.x;
		data_record1[cnt++] = vmc_all.pos_n.x;// robotwb.cog_pos_n.x;
		data_record1[cnt++] = vmc_all.spd_n.x; //robotwb.cog_spd_n.x;

		data_record1[cnt++] = robotwb.exp_pos_n.z;
		data_record1[cnt++] = vmc_all.pos_n.z;// robotwb.cog_pos_n.z;
		data_record1[cnt++] = vmc_all.spd_n.z;// robotwb.cog_spd_n.z;

		data_record1[cnt++] = vmc_all.ground_att_est[PITr];// robotwb.ground_att_cmd[PITrw];
		data_record1[cnt++] = vmc_all.ground_att_est[ROLr];//robotwb.ground_att_cmd[ROLrw];

		data_record1[cnt++] = robotwb.exp_spd_n.x;// -robotwb.exp_spd_b.x;
		data_record1[cnt++] = robotwb.exp_spd_n.z;//robotwb.exp_spd_b.z;
//-----------
		cnt = 0;
		data_record2[cnt++] = robotwb.Leg[0].tar_force_dis_n.y;
		data_record2[cnt++] = robotwb.Leg[0].force_est_n_output.y;
		data_record2[cnt++] = robotwb.Leg[0].tar_force_dis_n.z;
		data_record2[cnt++] = robotwb.Leg[0].force_est_n_output.z;
		data_record2[cnt++] = robotwb.Leg[1].tar_force_dis_n.y;
		data_record2[cnt++] = robotwb.Leg[1].force_est_n_output.y;
		data_record2[cnt++] = robotwb.Leg[1].tar_force_dis_n.z;
		data_record2[cnt++] = robotwb.Leg[1].force_est_n_output.z;

		data_record2[cnt++] = robotwb.Leg[0].is_ground;
		data_record2[cnt++] = robotwb.Leg[0].is_touch_est;
		data_record2[cnt++] = robotwb.Leg[1].is_ground;
		data_record2[cnt++] = robotwb.Leg[1].is_touch_est;

		data_record2[cnt++] = robotwb.Leg[0].trig_state;
		data_record2[cnt++] = robotwb.Leg[1].trig_state;

		data_record2[cnt++] = robotwb.Leg[0].tar_sita[0];
		data_record2[cnt++] = robotwb.Leg[0].tar_sita[1];
		data_record2[cnt++] = robotwb.Leg[0].sita[0];
		data_record2[cnt++] = robotwb.Leg[0].sita[1];
		data_record2[cnt++] = vmc[0].param.tar_epos_h.x;// robotwb.Leg[0].tar_epos_h.x;
		data_record2[cnt++] = vmc[0].param.tar_epos_h.z;// robotwb.Leg[0].tar_epos_h.z;
		data_record2[cnt++] = robotwb.Leg[0].epos_h.x;
		data_record2[cnt++] = robotwb.Leg[0].epos_h.z;
		//printf("%f\n", robotwb.Leg[0].tar_epos_h.z);
		data_record2[cnt++] = robotwb.Leg[1].tar_sita[0];
		data_record2[cnt++] = robotwb.Leg[1].tar_sita[1];
		data_record2[cnt++] = robotwb.Leg[1].sita[0];
		data_record2[cnt++] = robotwb.Leg[1].sita[1];
		data_record2[cnt++] = vmc[1].param.tar_epos_h.x;// robotwb.Leg[0].tar_epos_h.x;
		data_record2[cnt++] = vmc[1].param.tar_epos_h.z;// robotwb.Leg[0].tar_epos_h.z;
		data_record2[cnt++] = robotwb.Leg[1].epos_h.x;
		data_record2[cnt++] = robotwb.Leg[1].epos_h.z;


		cnt = 0;//临时记录
		float spd_gps = 0;// wb_gps_get_speed(GPS);jilu

		data_record3[cnt++] = slip_mode.pos.x;
		data_record3[cnt++] = slip_mode.pos.z;
		data_record3[cnt++] = 0;
		data_record3[cnt++] = slip_mode.vel.x;
		data_record3[cnt++] = slip_mode.vel.z;
		data_record3[cnt++] = 0;
		data_record3[cnt++] = slip_mode.force.x;
		data_record3[cnt++] = slip_mode.force.z;

		data_record3[cnt++] = spd_gps*0;
	    data_record3[cnt++] = vmc_all.spd_n.x;
		data_record3[cnt++] = gait_bound.slip_param.vd;

		data_record3[cnt++] = vmc[0].spd_n.x;
		data_record3[cnt++] = vmc[1].spd_n.x;


		cnt = 0;//学习用数据
#if 1
		static float temp[10] = {0};
		DigitalLPF(walk_gait.stable_value, &temp[0],5, dt);
		DigitalLPF(walk_gait.support_area, &temp[1],5, dt);
		data_record4[cnt++] = temp[0];
		data_record4[cnt++] = temp[1];

		data_record4[cnt++] = walk_gait.now_st_cog_n.x;
		data_record4[cnt++] = walk_gait.now_st_cog_n.y;
		//printf(" walk_gait.now_zmp_b.x=%f\n", walk_gait.now_st_cog_n.x);
		for (int i = 0; i < 4; i++) {
			if(gait.state[i]>1)
				data_record4[cnt++] = 2;
			else
				data_record4[cnt++] = gait.state[i];
			data_record4[cnt++] = gait.ground_flag[i][0];
			data_record4[cnt++] = gait.imp_force[i][Zr];
		}

#endif
		data_record4[cnt++] = vmc_all.spd_n.x;
		data_record4[cnt++] = vmc_all.spd_n.y;
		data_record4[cnt++] = vmc_all.spd_n.z;

		data_record4[cnt++] = vmc_all.att_rate[PITr];
		data_record4[cnt++] = vmc_all.att_rate[ROLr];
		data_record4[cnt++] = vmc_all.att_rate[YAWr];
		int i = 2;
		data_record4[cnt++] = vmc[i].sita1;
		data_record4[cnt++] = vmc[i].sita2;
		data_record4[cnt++] = vmc[i].dsita1;
		data_record4[cnt++] = vmc[i].dsita2;
		data_record4[cnt++] = vmc[i].epos.x;
		data_record4[cnt++] = vmc[i].epos.y;
		data_record4[cnt++] = vmc[i].epos.z;
		data_record4[cnt++] = vmc[i].spd.x;
		data_record4[cnt++] = vmc[i].spd.y;
		data_record4[cnt++] = vmc[i].spd.z;
		i = 0;
		data_record4[cnt++] = vmc[i].sita1;
		data_record4[cnt++] = vmc[i].sita2;
		data_record4[cnt++] = vmc[i].dsita1;
		data_record4[cnt++] = vmc[i].dsita2;
		data_record4[cnt++] = vmc[i].epos.x;
		data_record4[cnt++] = vmc[i].epos.y;
		data_record4[cnt++] = vmc[i].epos.z;
		data_record4[cnt++] = vmc[i].spd.x;
		data_record4[cnt++] = vmc[i].spd.y;
		data_record4[cnt++] = vmc[i].spd.z;
		i = 1;
		data_record4[cnt++] = vmc[i].sita1;
		data_record4[cnt++] = vmc[i].sita2;
		data_record4[cnt++] = vmc[i].dsita1;
		data_record4[cnt++] = vmc[i].dsita2;
		data_record4[cnt++] = vmc[i].epos.x;
		data_record4[cnt++] = vmc[i].epos.y;
		data_record4[cnt++] = vmc[i].epos.z;
		data_record4[cnt++] = vmc[i].spd.x;
		data_record4[cnt++] = vmc[i].spd.y;
		data_record4[cnt++] = vmc[i].spd.z;
		i = 3;
		data_record4[cnt++] = vmc[i].sita1;
		data_record4[cnt++] = vmc[i].sita2;
		data_record4[cnt++] = vmc[i].dsita1;
		data_record4[cnt++] = vmc[i].dsita2;
		data_record4[cnt++] = vmc[i].epos.x;
		data_record4[cnt++] = vmc[i].epos.y;
		data_record4[cnt++] = vmc[i].epos.z;
		data_record4[cnt++] = vmc[i].spd.x;
		data_record4[cnt++] = vmc[i].spd.y;
		data_record4[cnt++] = vmc[i].spd.z;
		data_record4[cnt++] = vmc[2].is_touch_mea;
		data_record4[cnt++] = vmc[0].is_touch_mea;
		data_record4[cnt++] = vmc[3].is_touch_mea;
		data_record4[cnt++] = vmc[1].is_touch_mea;

		cnt = 0;//步态相序 hunman
		if (isnan(robotwb.Leg[0].swingStates)|| isnan(robotwb.Leg[1].swingStates)|| isnan(robotwb.Leg[2].swingStates)|| isnan(robotwb.Leg[3].swingStates)) {
			data_record5[cnt++] = 0;
			data_record5[cnt++] = 0;
			data_record5[cnt++] = 0;
			data_record5[cnt++] = 0;
			data_record5[cnt++] = 0;
			data_record5[cnt++] = 0;
			data_record5[cnt++] = 0;
			data_record5[cnt++] = 0;

			data_record5[cnt++] = 0;
		}
		else {
			data_record5[cnt++] = vmc[0].sw_phase;// robotwb.Leg[0].swingStates;
			data_record5[cnt++] = robotwb.Leg[0].contactStates;
			data_record5[cnt++] = robotwb.Leg[1].swingStates;
			data_record5[cnt++] = robotwb.Leg[1].contactStates;
			data_record5[cnt++] = robotwb.Leg[2].swingStates;
			data_record5[cnt++] = robotwb.Leg[2].contactStates;
			data_record5[cnt++] = robotwb.Leg[3].swingStates;
			data_record5[cnt++] = robotwb.Leg[3].contactStates;

			data_record5[cnt++] = _hu_model.gait_mode;
		}
		data_record5[cnt++] = mom_lip[Yr].L_tar;
		data_record5[cnt++] = mom_lip[Yr].L_t;
		data_record5[cnt++] = mom_lip[Yr].L_t_kf;
		data_record5[cnt++] = mom_lip[Yr].L_est;
		data_record5[cnt++] = vmc_all.spd_n.y; 
		data_record5[cnt++] = vmc_all.spd_n.z;
		break;
	}
}
#endif